from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch.actions import ExecuteProcess

def generate_launch_description():
    share_dir = get_package_share_directory('Stewart_only_description')

    xacro_file = os.path.join(share_dir, 'urdf', 'Stewart_only.xacro')
    robot_description_config = xacro.process_file(xacro_file)
    robot_urdf = robot_description_config.toxml()


    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[
            {'robot_description': robot_urdf}
        ]
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher'
    )

    gazebo_server = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('gazebo_ros'),
                'launch',
                'gzserver.launch.py'
            ])
        ]),
        launch_arguments={
            # 'pause': 'true',
            'world': os.path.join(share_dir, 'world', 'world.world')
        }.items()
        
    )

    gazebo_client = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('gazebo_ros'),
                'launch',
                'gzclient.launch.py'
            ])
        ])
    )

    urdf_spawn_node = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=[
            '-entity', 'Stewart_only',
            '-topic', 'robot_description',
           
        ],
        output='screen'
    )

    load_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'stewart_rod_state_broadcaster'],
        output='screen'
    )

    delayed_load_controller = TimerAction(
        period=3.0,
        actions=[load_controller]
    )
    rod_load_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'rod_velocity_controller'],
        output='screen'
    )

    delayed_load_controller2 = TimerAction(
        period=3.0,
        actions=[rod_load_controller]
    )

    """外接控制逻辑"""
    velocity_publisher_node = Node(
        package='Stewart_only_description',
        executable='velocity_publisher',
        name='velocity_publisher',
        output='screen'
    )

    position_control_node = Node(
        package='Stewart_only_description',
        executable='position_control.py',
        name='position_control',
        output='screen'
    )

    recorder_node = Node(
        package='Stewart_only_description',
        executable='recorder.py',
        name='recorder',
        output='screen'
    )

    udp_interface_node = Node(
        package='Stewart_only_description',
        executable='UDP_interface.py',
        name='UDP_interface',
        output='screen'
    )

    stewart_inverse_kinematics_node = Node(
        package='Stewart_only_description',
        executable='stewart_inverse_kinematics',
        name='stewart_inverse_kinematics',
        output='screen'
    )

    main_logic_node = Node(
        package='Stewart_only_description',
        executable='main_logic.py',
        name='main_logic',
        output='screen'
    )


    return LaunchDescription([
        robot_state_publisher_node,
        gazebo_server,
        gazebo_client,
        urdf_spawn_node,
        # delayed_load_controller,
        # delayed_load_controller2,
        # position_control_node,
        # recorder_node,
        # udp_interface_node,
        # stewart_inverse_kinematics_node,
        # main_logic_node,
    ])
